
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_ekf.cpp
  * @author     baiyang
  * @date       2021-10-31
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>

#include "ahrs_ekf.h"

#include <EKF/ekf.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <sensor_imu/sensor_imu.h>
#include <sensor_baro/sensor_baro.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <perf/perf_counter.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void UpdateSystemFlagsSample(const uint64_t &timestamp);
/*----------------------------------variable----------------------------------*/
#if defined(HAL_NO_CCMRAM)
Ekf _ekf_g;
#else
Ekf _ekf_g RT_SECTION(".ccmram");
#endif

Ekf *_ekf = nullptr;
parameters *_params = nullptr;

// time slip monitoring
uint64_t _integrated_time_us = 0;    ///< integral of gyro delta time from start (uSec)
uint64_t _start_time_us = 0;         ///< system time at EKF start (uSec)
int64_t  _last_time_slip_us = 0;     ///< Last time slip (uSec)

static itc_node_t vehicle_gndeffect_sub;
static itc_node_t vehicle_status_sub;

uint32_t _device_id_accel;
uint32_t _device_id_baro;
uint32_t _device_id_gyro;
uint32_t _device_id_mag;

int32_t _param_ekf2_aid_mask;

perf_counter_t _ekf_elapsed_perf;
perf_counter_t _ekf_interval_perf;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_init()
{
    _ekf = &_ekf_g;
    _params = _ekf->getParamHandle();

    ahrs_ekf_assign_param();

    vehicle_gndeffect_sub = itc_subscribe(ITC_ID(vehicle_gndeffect), NULL);
    vehicle_status_sub = itc_subscribe(ITC_ID(vehicle_status), NULL);

    _ekf_elapsed_perf = perf_alloc(PC_ELAPSED, "ekf: ET");
    _ekf_interval_perf = perf_alloc(PC_INTERVAL, "ekf: INR");
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        按100Hz调用
  */
static bool ekf_update_imu_sample(imuSample &imu_sample_new)
{
    Vector3f_t delta_angle;
    Vector3f_t delta_velocity;

    uint8_t gyro_index = sensor_imu_get_primary_accel();
    uint8_t accel_index = sensor_imu_get_primary_gyro();

    bool update = sensor_imu_get_delta_angle(gyro_index, &delta_angle, &imu_sample_new.delta_ang_dt) 
                  && sensor_imu_get_delta_velocity(accel_index, &delta_velocity, &imu_sample_new.delta_vel_dt);

    uint8_t accel_clipping = sensor_imu_get_accel_clipping(accel_index);

    if (update) {
        imu_sample_new.time_us = sensor_imu_get_last_update_usec();
        imu_sample_new.delta_ang = Vector3f{delta_angle.x, delta_angle.y, delta_angle.z};
        imu_sample_new.delta_vel = Vector3f{delta_velocity.x , delta_velocity.y, delta_velocity.z};
        imu_sample_new.delta_vel_clipping[0] = accel_clipping & INS_CLIPPING_X;
        imu_sample_new.delta_vel_clipping[1] = accel_clipping & INS_CLIPPING_Y;
        imu_sample_new.delta_vel_clipping[2] = accel_clipping & INS_CLIPPING_Z;
    }

    if (_device_id_accel != sensor_imu_get_primary_accel_id()) {
        _device_id_accel = sensor_imu_get_primary_accel_id();
        _ekf->resetAccelBias();
    }

    if (_device_id_gyro != sensor_imu_get_primary_gyro_id()) {
        _device_id_gyro = sensor_imu_get_primary_gyro_id();
        _ekf->resetGyroBias();
    }

    return update;
}

/**
  * @brief       运行扩展卡尔曼滤波器
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  * 
  */
void ahrs_ekf_update()
{
    perf_begin(_ekf_elapsed_perf);
    perf_count(_ekf_interval_perf);

    bool imu_updated = false;
    imuSample imu_sample_new {};

    uint64_t imu_dt = 0; // for tracking time slip later

    imu_updated = ekf_update_imu_sample(imu_sample_new);
    imu_dt = imu_sample_new.delta_ang_dt*1e6;

    if (imu_updated) {
        const uint64_t now = imu_sample_new.time_us;
        _ekf->setIMUData(imu_sample_new);

        ahrs_ekf_publish_attitude(now);

        // integrate time to monitor time slippage
        if (_start_time_us > 0) {
            _integrated_time_us += imu_dt;
            _last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;

        } else {
            _start_time_us = imu_sample_new.time_us;
            _last_time_slip_us = 0;
        }

        ahrs_ekf_update_baro_sample();
        ahrs_ekf_update_compass_sample();
        ahrs_ekf_update_gps_sample();
        ahrs_ekf_update_airspeed_sample();

        UpdateSystemFlagsSample(now);

        // run the EKF update and output
        if (_ekf->update()) {
            ahrs_ekf_publish_local_position(now);
            ahrs_ekf_publish_global_position(now);
            ahrs_ekf_publish_innovations(now);
            ahrs_ekf_publish_status(now);
            ahrs_ekf_publish_wind_estimate(now);
            ahrs_ekf_publish_sensor_bias(now);
            ahrs_ekf_publish_yaw_estimator_status(now);
        }
    }

    perf_end(_ekf_elapsed_perf);
}

static void UpdateSystemFlagsSample(const uint64_t &timestamp)
{
    // EKF system flags
    if (itc_update(vehicle_gndeffect_sub) || itc_update(vehicle_status_sub)) {

        estimator::systemFlagUpdate flags{};
        flags.time_us = timestamp;

        // vehicle_status
        uitc_vehicle_status vehicle_status;

        if ((itc_copy(ITC_ID(vehicle_status), vehicle_status_sub, &vehicle_status) == 0)
            && (timestamp < vehicle_status.timestamp_us + 3*1000*1000)) {

            // initially set in_air from arming_state (will be overridden if land detector is available)
            flags.in_air = vehicle_status.armed;

            // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
            flags.is_fixed_wing = (vehicle_status.vehicle_type == UITC_VEHICLE_STATUS_VEHICLE_TYPE_FIXED_WING);
        }

        // vehicle_land_detected
        uitc_vehicle_gndeffect gndeffect;
        if ((itc_copy(ITC_ID(vehicle_gndeffect), vehicle_gndeffect_sub, &gndeffect) == 0)
            && (timestamp < gndeffect.timestamp_us + 3 * 1000 * 1000)) {

            flags.at_rest = gndeffect.at_rest;
            flags.in_air = !gndeffect.landed;
            flags.gnd_effect = gndeffect.in_ground_effect;
        }

        _ekf->setSystemFlagData(flags);
    }
}

/*------------------------------------test------------------------------------*/


